//
// Created by daybeha on 2022/7/12.
//

#include "ros/ros.h"
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include "std_msgs/builtin_float.h"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"

using namespace std;


bool flag = false;
void receiveScore(const std_msgs::Float32ConstPtr &msg) {
//    ROS_INFO("订阅的数据:%s", msg->data.c_str());
    cout << "receive score: " << msg->data << endl;
    flag = false;
}

int main(int argc, char *argv[]){
//    setlocale(LC_CTYPE, "zh_CN.utf8");
    setlocale(LC_ALL, "");

    //初始化ros节点
    ros::init(argc,argv,"talker");
//    //创建ros节点句柄（not necessary）
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("/image", 1);

//    ros::Publisher pub = nh.advertise<sensor_msgs::Image>("/image", 2);
    ros::Subscriber sub = nh.subscribe<std_msgs::Float32>("/score", 1, receiveScore);


//    std::string msg_front = "hello --> ";
    int count = 1;
//
    ros::Rate r(1);
//
////    ros::Duration(3.0).sleep();
    r.sleep();
    while (ros::ok()){
        if(!flag){
            stringstream ss0, ss1;
            ss0 << setfill('0') << setw(6) << count;
            // 读取图片
            cv::Mat img0 = cv::imread("/home/daybeha/Documents/Dataset/Kitti/sequences/00/image_0/"+  ss0.str() +".png",
                                      cv::IMREAD_GRAYSCALE);
            sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img0).toImageMsg();
            // 发布图像
            pub.publish(msg);
            cout << "published image " << count << endl;
            count++;
            flag= true;
        }


        ros::spinOnce();
        r.sleep();


        if(count>10){
            ros::shutdown();  // 关闭节点
        }
    }

    return 0;
}


